#include "ros/ros.h" 
#include "std_msgs/String.h"
#include <image_transport/image_transport.h>
#include <camera_info_manager/camera_info_manager.h>
#include <sensor_msgs/CameraInfo.h>

#include <highgui.h>
#include "cxcore.hpp"
#include <pthread.h>
#include <sys/time.h>
#include <unistd.h>

#include <stdio.h>
#include <stdlib.h>
#include <string.h>


#include <sstream>

#include "loitordriver.h"

using namespace std;
using namespace cv;


/**cmaera calibration infomation**/
String left_info_url("package://loitor_ros/calibrations/left_camera.yaml");
String right_info_url("package://loitor_ros/calibrations/right_camera.yaml");

int main(int argc, char **argv)
{ 
  ros::init(argc, argv, "loitor_stereo_visensor");
  ros::NodeHandle nh;
  //load the camera setting
  if(argv[1])visensor_load_settings(argv[1]);
  else visensor_load_settings("Loitor_VISensor_Setups.txt");

  float hardware_fps=visensor_get_hardware_fps();
  // 使用camera硬件帧率设置发布频率
  ros::Rate loop_rate((int)hardware_fps);
  loitor::loitordriver loitorcamera(nh);

  while (ros::ok())
  {
    cout <<"??"<<endl;
    loitorcamera.fill_image(visensor_cam_selection);

    ros::spinOnce();
    loop_rate.sleep();
  }
  return 0;
}











